COMMENT ⊗   VALID 00019 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00003 00002		TITLE SERVO
C00006 00003	Shared ram.
C00009 00004	Start of prom.
C00012 00005	Sets the current position to the converted encoder value, the
C00014 00006	RSTCKW:	LDAI	377	Reset check word.
C00017 00007	Clock tick interrupt.
C00019 00008	   Interpolate the velocity.
C00021 00009		BITZ	MODE	If servo is disabled, we're
C00023 00010	INBAND:	LDAI	LSBENB	In band. Is LSB servo enabled?
C00026 00011		LDAZ	IVEL-1	Round IVEL by seting the carry to the complement
C00028 00012	DAC output routine.
C00031 00013	CMDTBL:		DEFERRED COMMAND TABLE.
C00034 00014	   INTINC ← CMDVEL - IVEL. (Velocity difference).
C00036 00015	Position conversion routine.
C00038 00016	Arithmetic routines.
C00041 00017	Enter with sign and characteristic in A, mantissa in Y
C00044 00018	Enter with characteristic of multiplier in A,
C00047 00019	DAC output table.
C00050 ENDMK
C⊗;
	TITLE SERVO
	.INSERT ASMBL.FAI[CMS,LCS]

;I/O address definitions.
   DAC ← 100000	;8 bit DAC.
   JCR ← 120000	;Joint control output register.
   ENCL ← 140000	;Encoder mux low.
   ENCH ← 140001	;Encoder mux high.

   STKSIZ ← 377	;Stack size.
   LSBENB ← 40	;Enable LSB servo.

;Zero page variables.
;Not shared.

DSPAT:	BLOCK 2	;Dispatch address for commands.
DEFCMD:	0	;Deferred command.
SAVPOS:	BLOCK 2	;Position for deferred servo command.

CMDVEL:	BLOCK 2	;Commanded velocity.
CURVEL:	BLOCK 2	;Current velocity.
	0	;SETINC-1.
SETINC:	BLOCK 2	;Interpolating increment for setpoints.
	0	;INTINC-1.
INTINC:	BLOCK 2	;Interpolating increment for velocity.
OLDSP:	BLOCK 2	;Last commanded setpoint, for CMDVEL.
POSERR:	BLOCK 2	;Current position error.

INCTR:	0	;Count the interpolations.
HSTTMR:	0	;Count ticks between host commands.

LOGTMP:	BLOCK 4	;Temp for the arithmetic routines.

	0	;IVEL-1.
IVEL:	BLOCK 2	;Interpolated velocity.
VPTR:	0	;Velocity averaging index.
VELTBL:	BLOCK 20;Velocity averaging table.
FRICTN:	BLOCK 2	;Friction offset.
ACCEL:	BLOCK 2	;Acceleration term.

ZAPEND ← .-1	;Clear all the above in startup.

TL:	0	;Scratch for gray to binary.
TH:	0

MTMP:	BLOCK 2	;Copy of multipilcand from shared memory.
;Shared ram.
   LOC 200	;Second half of zero page.

;STATUS byte bits.
;	7	6	5	4	3	2	1	0
;    error   check    time     no      bad
;     flag    word     out    tick     pos
	0	;Locked.
STATUS:	0	;Flags for the host.

;MODE byte bits.
;Bit	7	6	5	4	3	2	1	0
;    servo   integ     lsb    diag
;     enlb    enlb    enbl    enbl
	0	;Locked.
MODE:	0	;Mode bits from host.

CKWORD:	BLOCK 2	;Host I/O check/command word.
CMDPOS:	BLOCK 2	;Commanded position from host.

;IOCTRL byte bits.
;Bit	7	6	5	4	3	2	1	0
;      in		      lsb   integ     pos
;     tol		     enlb  disabl    mode
	0	;Locked.
IOCTRL:	0	;Copy of JCR output port.

CURPOS:	BLOCK 2	;Current position.

	0	;Locked.
NINTER:	0	;# of interpolations between position commands.

	0	;Locked.
INTSCL:	0	;# of bits to shift setpoint dif for interpolating.

	0	;Locked.
HSTLIM:	0	;# of clock ticks allowed between host commands.

VGAIN:	BLOCK 2	;Velocity error gain.
GRAVTY:	BLOCK 2	;DC offset for gravity.
POSTOL:	BLOCK 2	;Half-width of position tolerance band.
INTTOL:	BLOCK 2	;Half-width of integration band.
PGAIN:	BLOCK 2	;Position error gain.
VELERR:	BLOCK 2	;Velocity error.
VSUM:	BLOCK 2	;Sum of last eight velocitys.

	0	;Locked.
FSCL:	0	;# of right shifts for FRICTN.

	BLOCK 2	;For SETPT-1.
SETPT:	BLOCK 2	;Current setpoint.

 	0	;Locked.
DACSIG:	0	;Last DAC output value.

ACGAIN:	BLOCK 2	;Acceleration gain.
;Start of prom.
   LOC 174000

INITBL:	STATUS	↔	0	;Reset error bits.
	NINTER	↔	=16	;Number of interpolations default.
	INTSCL	↔	4	;Interpolating scale default.
	HSTLIM	↔	=24	;Ticks before host time out default.
	FSCL	↔	=6	;Shifts for friction.
		;13,,100 is 40013 is 1.0.
	PGAIN	↔	13	;F.P. position gain. Lock.
	PGAIN+1	↔	100	;Unlock.
		;13,,275 is 136413 is -0.125.
	VGAIN	↔	13	;F.P. velocity gain. Lock.
	VGAIN+1	↔	275	;Unlock.
	ACGAIN	↔	0	;F.P. acceleration gain. Lock.
	ACGAIN+1↔	0	;Unlock.
	377	;End of INITBL flag.

;Power on reset.
START:	CLD
	LDXI	STKSIZ	;Setup stack.
	TXS

	LDAI	0
	STA	DAC	;Clear DAC.

	LDXI	ZAPEND
RLOOP:	STAZX	0	;Reset ram.
	DEX
	BPL	RLOOP

	LDXI	354	;-20.
ZSR:	STAZX	VGAIN+24;Clear shared ram.
	INX
	BMI	ZSR

	TAY	;Y ← 0.
	BEQ	RSTDEF	;Jump
DLOOP:	INY
	LDAY	INITBL	;Init ram.
	STAZX	0
	INY
RSTDEF:	LDXY	INITBL
	CPXI	377
	BNE	DLOOP
;Sets the current position to the converted encoder value, the
;setpoint the same, clears the setpoint interpolating increment,
;disables the servo, and waits for a command.
STOP:	SEI	;Go into stop mode.
	JSR	GETPOS	;Read encoder and convert to binary.
	STAZ	CURPOS	;Set the current position.
	STXZ	CURPOS+1;Unlock.
	STAZ	SETPT	;Set the setpoint.
	STXZ	SETPT+1	;Unlock.
	STAZ	OLDSP	;For CMDVEL.
	STXZ	OLDSP+1

	LDAI	71	;Reset I/O control bits. Position mode off.
	STAZ	IOCTRL
	STA	JCR
	LDAI	0
	STAZ	MODE	;Clear position servo enable, etc..
	STAZ	DEFCMD	;Clear the deferred command flag.
	STAZ	SETPT-1	;Clear the setpoint extension
	STAZ	SETINC-1;and the interpolator.
	STAZ	SETINC
	STAZ	SETINC+1

	LDXI	27
CLRVEL:	STAZX	IVEL-1	;Clear velocity, friction, and acceleration values.
	DEX
	BPL	CLRVEL

	STAZ	VELERR	;Clear velocity error and sum.
	STAZ	VELERR+1;Unlock.
	STAZ	VSUM	;Lock.
	STAZ	VSUM+1	;Unlock.

	CLI	;End of reset.
RSTCKW:	LDAI	377	;Reset check word.
	LDXI	0
	SEI
	STAZ	CKWORD	;Lock.
	STXZ	CKWORD+1;Unlock.
	CLI

;Idle loop. Wait for command.
IDLE:	LDAZ	CKWORD+1;Check for new check word.
	BEQ	IDLE	;Not equal if bit 7 is complement of low byte.

	SEC
	SEI
	ADCZ	CKWORD	;Lock.
	LDXZ	CKWORD+1;Unlock.
	CLI
	TAY
	BNE	CKWDER	;Check word error.

   ;Check here for immediate or deferred.
	TXA	;Check for valid command.
	ORAI	3	;3 for two commands and bit 0 = 0.
	ADCI	0	;Carry = 1.
	BNE	CKWDER	;Not a valid command.

   ;Valid host command.
	LDAZ	HSTLIM	;Reset host timer.
	STAZ	HSTTMR
	LDAZ	DEFCMD	;Check if no TICK?
	BNE	NOTICK	;No response since last deferred command.

   ;Check here if posiition command?
	SEI
	LDYZ	CMDPOS	;Read position for servo command.
	LDAZ	CMDPOS+1;Unlock.
	CLI
	STYZ	SAVPOS	;Save it for later.
	STAZ	SAVPOS+1

	ASLA	;Check for valid position.
	BCS	CSET
	BMI	BADPOS
	BPL	GOODP	;Jump.
CSET:	BPL	BADPOS

GOODP:	STXZ	DEFCMD	;Save deferred command pointer.

	JMP	RSTCKW	;Handshake with host via CKWORD.

CKWDER:	LDAI	300	;Set check word error flag.
WSTAT:	ORAZ	STATUS
	STAZ	STATUS
	JMP	STOP

NOTICK:	LDAI	220	;Set tick error flag.
	BNE	WSTAT	;Jump.

BADPOS:	LDAI	210	;Set bad position error flag.
	BNE	WSTAT	;Jump.
;Clock tick interrupt.
TICK:	PHA	;Save state.
	TXA
	PHA
	TYA
	PHA

	INCZ	IOCTRL	;Turn on interrupt flag bit.
	LDAZ	IOCTRL	;This is only for timing checks.
	STA	JCR	;Can be flushed.

	JSR	GETPOS	;Read position and convert to binary.
	SEC
	SBCZ	CURPOS	;Subtract the old position
	STAZ	CURVEL	;yielding the velocity.
	TXA	;High byte of binary position.
	SBCZ	CURPOS+1;Unlock.
	STAZ	CURVEL+1

	STYZ	CURPOS	;Update the current position.
	STXZ	CURPOS+1;Unlock.

	DECZ	HSTTMR	;Count the ticks since the last command
	BPL	HOSTOK	;and check for timeout.

	LDAI	0	;Host dead. Stop.
	STAZ	HSTTMR
	STAZ	INTINC-1
	STAZ	INTINC
	STAZ	INTINC+1
	STAZ	FRICTN
	STAZ	FRICTN+1
	STAZ	ACCEL
	STAZ	ACCEL+1
	LDAI	240	;Set host time out flag
	ORAZ	STATUS
	STAZ	STATUS

	LDAI	20	;Check for diagnostic enable.
	BITZ	MODE
	BNE	HOSTOK	;If diagnostics, then servo anyway.

	LDAI	177	;Turn off servo enable.
	ANDZ	MODE
	STAZ	MODE

HOSTOK:	BITZ	MODE	;Check if servo is enabled.
	BMI	INTVEL
	JMP	CURSRV	;don't servo.
   ;Interpolate the velocity.
INTVEL:	CLC
	LDAZ	IVEL-1
	ADCZ	INTINC-1;IVEL ← IVEL + INTINC.
	STAZ	IVEL-1
	LDAZ	IVEL
	ADCZ	INTINC
	STAZ	IVEL
	LDAZ	IVEL+1
	ADCZ	INTINC+1
	STAZ	IVEL+1

   ;Interpolate the setpoints.
INTRS:	CLC
	LDAZ	SETPT-1	;Unlock.
	ADCZ	SETINC-1;Add the increment to the setpoint.
	STAZ	SETPT-1	;Unlock.
	LDAZ	SETPT	;Lock.
	ADCZ	SETINC
	TAX	;Save SETPT for lockout.
	LDAZ	SETPT+1	;Unlock.
	ADCZ	SETINC+1
	STXZ	SETPT	;Lock.
	STAZ	SETPT+1	;Unlock.

	DECZ	INCTR	;Check if this is the last interpolation.
	BNE	GPOSER

	LDAI	0	;Clear SETINC if done interpolating.
	STAZ	SETINC-1
	STAZ	SETINC
	STAZ	SETINC+1
	STAZ	INTINC-1;Clear INTINC (commanded velocity).
	STAZ	INTINC
	STAZ	INTINC+1

;Calculate the position error.
GPOSER:	SEC
	LDAZ	CURPOS	;POSERR ← CURPOS - SETPT.
	LDXZ	CURPOS+1;Unlock.
	SBCZ	SETPT	;Lock.
	STAZ	POSERR
	TXA	;Get CURPOS+1.
	SBCZ	SETPT+1	;Unlock.
	STAZ	POSERR+1
	BITZ	MODE	;If servo is disabled, we're
	BPL	OOTOL	;automatically out of tolerance

	LDAZ	POSERR+1;Test the sign of pos error.
	BMI	NEGPER

	LDAZ	POSTOL	;Positive. Compare with tol.
	CMPZ	POSERR
	LDAZ	POSTOL+1;Unlock.
	SBCZ	POSERR+1
	BCS	TOLOK	;In tolerance.
	BCC	OOTOL	;Jump.

NEGPER:	CLC	;Negative. Add the tolerance.
	LDAZ	POSTOL	;Lock.
	ADCZ	POSERR
	LDAZ	POSTOL+1;Unlock.
	ADCZ	POSERR+1
	BCS	TOLOK	;In tolerance.

OOTOL:	LDAZ	IOCTRL	;Out of tolerance.
	ANDI	177	;Turn off the in tolerance
	BNE	WCNTRL	;indicator. Jump.

TOLOK:	LDAZ	IOCTRL	;In tolerance. Turn it on.
	ORAI	200
WCNTRL:	STAZ	IOCTRL
	STA	JCR	;Copy it to output.

	BITZ	MODE	;If intergration is disabled,
	BVC	OOBAND	;turn it off.
	LDAZ	POSERR+1;Test sign of position error.
	BMI	ADTOL

	LDAZ	INTTOL	;Positive. Compare with tol.
	CMPZ	POSERR
	LDAZ	INTTOL+1;Unlock.
	SBCZ	POSERR+1
	BCS	INBAND	;In band. Turn on integrator.
	BCC	OOBAND	;Jump.

ADTOL:	CLC	;Negative. Add the tolerance.
	LDAZ	INTTOL	;Lock.
	ADCZ	POSERR
	LDAZ	INTTOL+1;Unlock.
	ADCZ	POSERR+1
	BCS	INBAND	;Check if in band.

OOBAND:	LDAZ	IOCTRL	;Out of band. Turn off
	ORAI	10	;integration by setting the
	ANDI	357	;control bit. LSB servo off.
	BNE	WCTRL2	;Jump.
INBAND:	LDAI	LSBENB	;In band. Is LSB servo enabled?
	BITZ	MODE
	BEQ	RCNTRL
	LDAZ	POSERR	;Yes. Is the error exactly 0?
	ORAZ	POSERR+1
	BNE	RCNTRL

	LDAZ	IOCTRL	;It is. Integration off, LSB
	ORAI	30	;servo on.
	BNE	WCTRL2	;Jump.

RCNTRL:	LDAZ	IOCTRL	;LSB disabled or error not zero.
	ANDI	347	;LSB servo off, integration on.
WCTRL2:	STAZ	IOCTRL
	STA	JCR	;Output it.

	LDXZ	PGAIN	;Copy position gain for multiply.
	LDYZ	PGAIN+1	;Unlock.
	STXZ	MTMP
	STYZ	MTMP+1

	LDYZ	POSERR
	LDAZ	POSERR+1
	JSR	LOG	;Float the position error.
	LDXI	MTMP	;Point X to copy of PGAIN.
	JSR	MULTIP	;POSERR ← POSERR * PGAIN.
	JSR	EXP	;Fix it.
	STYZ	POSERR
	STAZ	POSERR+1

;Get the velocity error.
	CLC
	LDAZ	VSUM	;Lock.
	ADCZ	CURVEL	;VSUM ← VSUM + CURVEL.
	TAX
	LDAZ	VSUM+1	;Unlock.
	ADCZ	CURVEL+1
	TAY
	TXA
	LDXZ	VPTR	;Get velocity averaging index.
	SEC
	SBCZX	VELTBL	;VSUM ← VSUM - VELTBL[VPTR].
	STAZ	VSUM	;Lock.
	TYA
	SBCZX	VELTBL+10
	STAZ	VSUM+1	;Unlock.

	LDAZ	CURVEL	;VELTBL[VPTR] ← CURVEL.
	STAZX	VELTBL
	LDAZ	CURVEL+1
	STAZX	VELTBL+10

	INX	;VPTR ← (VPTR + 1) .AND. (VTLEN - 1).
	TXA
	ANDI	7
	STAZ	VPTR
	LDAZ	IVEL-1	;Round IVEL by seting the carry to the complement
	EORI	200	;of bit 7 in IVEL-1.
	ROLA

	LDAZ	VSUM	;Lock.
	LDXZ	VSUM+1	;Unlock.

	SBCZ	IVEL	;VELERR ← VSUM - IVEL.
	TAY	;Save VELERR.
	STAZ	VELERR	;Lock.
	TXA	;Get VSUM+1.
	SBCZ	IVEL+1
	STAZ	VELERR+1;Unlock.

	JSR	LOG	;Float the velocity error.
	STYZ	MTMP	;Save the F.P. VELERR.
	STAZ	MTMP+1

	LDYZ	VGAIN	;Get the velocity gain.
	LDAZ	VGAIN+1	;Unlock.
	LDXI	MTMP	;multiply by the velocity error,
	JSR	MULTIP	;VELERR * VGAIN.
	JSR	EXP	;Fix it.
	TAX	;Save high byte.

	CLC
	TYA	;Get low byte.
	ADCZ	POSERR	;Add the position error...
	TAY	;Save low byte.
	TXA	;Get high byte.
	ADCZ	POSERR+1
	TAX	;Save high byte.

	CLC
	TYA	;Get low byte.
	ADCZ	FRICTN	;...the friction offset...
	TAY	;Save low byte.
	TXA	;Get high byte.
	ADCZ	FRICTN+1
	TAX	;Save high byte.

	CLC
	TYA	;Get low byte.
	ADCZ	ACCEL	;...the acceleration term...
	TAY	;Save low byte.
	TXA	;Get high byte.
	ADCZ	ACCEL+1
	STAZ	POSERR+1

	CLC	;...and the gravity offset.
	TYA	;Get the low byte.
	ADCZ	GRAVTY	;Lock.
	TAY	;Save low byte.
	LDAZ	GRAVTY+1;Unlock.
	ADCZ	POSERR+1
;DAC output routine.
;Enter with 2 byte value in Y (low), A (high).
	BMI	NEGDAC	;Assuming the last inst. loaded A.
	CPYI	200	;Positive. Compare with 2↑7.
	SBCI	0
	BCC	INRNGE

TOOHI:	LDYI	177	;Too high. Saturate positive.
	BNE	INRNGE	;Jump.

NEGDAC:	CPYI	200	;Negative. Compare with -2↑7.
	SBCI	377
	BCS	INRNGE

TOOLOW:	LDYI	200	;Too low. Saturate to -2↑7.

INRNGE:	LDAY	VETBL	;Straighting it.
	STAZ	DACSIG	;Unlock.
	STA	DAC	;Output 8 bits to the DAC.

CMDSP:	LDAZ	DEFCMD	;Check for a command.
	BEQ	INTXIT
	ANDI	2	;Low nibble command bit.
	TAX
	LDAX	CMDTBL	;Get command address.
	STAZ	DSPAT
	LDAX	CMDTBL+1
	STAZ	DSPAT+1
	JMPIN	DSPAT	;Execute command.

CMDEND:	LDAI	0	;Done with deferred command.
	STAZ	DEFCMD	;Reset command word.
INTXIT:	DECZ	IOCTRL	;Turn off interrupt flag.
	LDAZ	IOCTRL	;Can be flushed.
	STA	JCR
	PLA	;Restore state and dismiss interrupt.
	TAY
	PLA
	TAX
	PLA
	RTI

;Servo disabled.
CURSRV:	LDAI	0
	STA	DAC	;Turn off the servo valve.
	STAZ	SETPT-1	;Make the setpoint track
	LDAZ	CURPOS	;the current position in order to
	STAZ	SETPT	;keep the arm from twitching when
	LDAZ	CURPOS+1;the host enables the servo. Unlock.
	STAZ	SETPT+1	;Unlock.

	LDAI	373
	ANDZ	IOCTRL	;Turn off position mode bit.
	STAZ	IOCTRL
	STA	JCR
	JMP	CMDSP	;Go check on commands.
CMDTBL:		;DEFERRED COMMAND TABLE.
	CMDEND∧377	;Nop.
	(CMDEND⊗-10)∧377
	CMDSRV∧377	;Servo command.
	(CMDSRV⊗-10)∧377

;Deferred commands.
CMDSRV:	LDAZ	MODE	;Servo command.
	BMI	ENBLD	;Test for servo enabled.
	JMP	CMDEND	;No. End this command. 

ENBLD:	SEC	;Enabled.
	LDAZ	SAVPOS	;Get commanded position.
	SBCZ	SETPT	;Get difference between next position
	STAZ	SETINC	;and the last setpoint.
	LDAZ	SAVPOS+1
	SBCZ	SETPT+1	;Unlock.

	LDXI	0
	STXZ	SETPT-1	;Clear setpoint and increment extentions.
	STXZ	SETINC-1
	STXZ	INTINC-1

	LDXZ	INTSCL	;A = SETINC+1.
SCAL:	CMPI	200	;Extend sign.
	RORA	;Divide the difference by the number of interpolations.
	RORZ	SETINC
	RORZ	SETINC-1
	DEX
	BNE	SCAL
	STAZ	SETINC+1;Which yields the interpolating increment.

	LDAZ	NINTER
	STAZ	INCTR	;Setup the interpolator count.

	SEC
	LDAZ	SAVPOS
	SBCZ	OLDSP	;CMDVEL ← CMDPOS - OLDSP.
	STAZ	CMDVEL
	STAZ	FRICTN
	LDAZ	SAVPOS+1
	SBCZ	OLDSP+1
	TAY	;Save CMDVEL+1.

	LDXZ	FSCL	;Unlock.
FSCAL:	CMPI	200	;Extend sign and scale FRICTN by FSCL.
	RORA
	RORZ	FRICTN
	DEX
	BNE	FSCAL

	BCC	NOINC	;Round it.
	INCZ	FRICTN
	BNE	NOINC
	ADCI	0	;C = 1.
NOINC:	STAZ	FRICTN+1
   ;INTINC ← CMDVEL - IVEL. (Velocity difference).
	LDAZ	IVEL-1
	ROLA		;IVEL * 2.
	TAX	;Save it for rounding.
	LDAZ	IVEL
	ROLA
	STAZ	INTINC
	LDAZ	IVEL+1
	ROLA
	STAZ	INTINC+1

	TXA	;Get IVEL-1 * 2.
	BPL	COPYG	;Round IVEL * 2.
	INCZ	INTINC
	BNE	COPYG
	INCZ	INTINC+1

COPYG:	LDAZ	ACGAIN	;Copy this for multiply.
	LDXZ	ACGAIN+1;Unlock.
	STAZ	MTMP
	STXZ	MTMP+1

	SEC
	LDAZ	CMDVEL
	SBCZ	INTINC	;INTINC ← CMDVEL - (IVEL * 2).
	STAZ	INTINC
	TYA	;Get CMDVEL+1.
	SBCZ	INTINC+1
	STAZ	INTINC+1

	LDYZ	INTINC	;A = INTINC+1.
	JSR	LOG	;Float the velocity difference.

	LDXI	MTMP	;Point X to copy of ACGAIN.
	JSR	MULTIP	;VELDIF * ACGAIN.
	JSR	EXP	;Fix it.
	STYZ	ACCEL	;ACCEL ← INTINC * ACGAIN.
	STAZ	ACCEL+1

	LDAZ	INTINC+1;INTINC ← (CMDVEL - (IVEL * 2)) / 32.
	LDXZ	INTSCL
ISCAL:	CMPI	200	;Extend sign and divide by the number of
	RORA		;interpolations + 1.
	RORZ	INTINC
	RORZ	INTINC-1
	DEX
	BPL	ISCAL
	STAZ	INTINC+1

	LDAZ	SAVPOS
	STAZ	OLDSP	;OLDSP ← CMDPOS.
	LDAZ	SAVPOS+1
	STAZ	OLDSP+1

	LDAI	44
	ORAZ	IOCTRL	;Turn on servo and current mode enable bits.
	STAZ	IOCTRL
	STA	JCR	;Output it.
	JMP	CMDEND
;Position conversion routine.
GETPOS:	LDY	ENCL	;Read encoder.
	LDA	ENCH
;Convert from gray to binary.
	STAZ	TH
	LSRA	;Shift by 1.
	EORZ	TH
	STAZ	TH
	TAX	;X ← high byte.

	TYA
	STAZ	TL
	RORA
	EORZ	TL
	STAZ	TL

	LSRZ	TH	;Shift by 2.
	RORA
	LSRZ	TH
	RORA
	EORZ	TL
	STAZ	TL
	TAY	;Y ← low byte.

	TXA	;Get high byte.
	EORZ	TH
	STAZ	TH

	LSRA	;Shift by 4.
	RORZ	TL
	LSRA
	RORZ	TL
	LSRA
	RORZ	TL
	LSRA
	RORZ	TL

	EORZ	TH
	STAZ	TH
	TYA
	EORZ	TL
	EORZ	TH	;Shift by 8.
	TAY	;Save low byte.

	LDXZ	TH	;Get high byte.
	BITZ	TH
	BVC	POS	;Check if negative.
	TXA
	ORAI	200	;Extend sign.
	TAX

POS:	TYA	;Returns with position in A, Y (low) and X (high).
	RTS
;Arithmetic routines.
;Enter with high byte in A, low in Y.
;Returns A = characteristic and sign, Y = mantissa.
;Clobbers X, LOGTMP, LOGTMP+1.
LOG:	STYZ	LOGTMP	;Save the inputs.
	STAZ	LOGTMP+1

	LDXI	20+100	;Init characteristic to 15.
	CMPI	0	;Test sign of input.
	BPL	POSIN
	SEC	;Negative. 2's complement it.
	LDAI	0
	SBCZ	LOGTMP
	STAZ	LOGTMP
	LDAI	0
	SBCZ	LOGTMP+1
POSIN:	BNE	NORML	;Is high byte zero?
	LDAZ	LOGTMP	;Yes. Low byte?
	BEQ	RTRN	;If so, return zero.
	LDYI	0	;Low nonzero. Shift left one
	STYZ	LOGTMP	;byte,
	LDXI	10+100	;change characteristic to 7.
NORML:	DEX	;Normalize the number, counting the
	ASLZ	LOGTMP	;characteristic down. When the
	ROLA	;first "1" shifts out, we've subtracted
	BCC	NORML	;1 from the normalized number
	ASLZ	LOGTMP	;(This rounds the result)
	ADCI	=11	;and are left with the fraction
	TAY	;Adding 11 to that is equivalent to
	TXA	;adding 0.043.
	ADCI	0	;Propagate the carry into the
			;characteristic.
	ASLA	;Insert the sign bit from the saved
	ASLZ	LOGTMP+1;input.
	RORA
RTRN:	RTS	;Done.
;Enter with sign and characteristic in A, mantissa in Y
;Returns 16-bit integer, low byte in Y, high in A.
;Clobbers X, LOGTMP, LOGTMP+1.
EXP:	STAZ	LOGTMP+1;Save sign of input.
	ANDI	177	;Mask it off.
	BEQ	ZEROIN	;Zero characteristic returns
	TAX	;zero.
	TYA	;Get the mantissa...
	SEC
	SBCI	=11	;...subtract 0.043...
	STAZ	LOGTMP	;(save this value)
	TXA	;...propagate the carry and get rid
	SBCI	100	;of the XS-64 offset.
	BMI	NEGIN	;If negative (value < 1.0)
			;return zero.
	CMPI	=15	;Test for overflow (value>=2↑15
	BCS	SATUR
	TAX	;...no. Number is in range.
	ADCI	370	;Is characteristic below 8?
	BMI	BLOATE
	TAX	;No. Reduce if by 8,
	JSR	UNNORM	;unnormalize.
	BMI	GETTMP	;Jump.

BLOATE:	JSR	UNNORM	;Yes. Unnormalize, then
	ASLZ	LOGTMP	;(round result)
	ADCI	0
	STAZ	LOGTMP	;use result as low byte and
	LDAI	0	;set high byte to zero.

GETTMP:	LDYZ	LOGTMP
GTMP1:	LDXZ	LOGTMP+1;Test sign of input...
	BPL	POSIGN
	STAZ	LOGTMP+1;...negative. 2's complement
	LDAI	0	;the result.
	SEC
	SBCZ	LOGTMP
	TAY
	LDAI	0
	SBCZ	LOGTMP+1
POSIGN:	RTS

NEGIN:	LDAI	0	;Set the result to zero if the
ZEROIN:	TAY	;input is negative.
	RTS

SATUR:	LDYI	377	;Saturate result to 2↑15 - 1 if
	STYZ	LOGTMP	;input was 15 or more.
	LDAI	177
	BNE	GTMP1	;Jump.

UNNORM:	LDAI	1	;Unnormalize subroutine. Add 1
	BNE	DECRX	;to the fraction. Jump.

SCALE:	ASLZ	LOGTMP	;Scale the fraction left by the
	ROLA	;amount of the characteristic.
DECRX:	DEX
	BPL	SCALE
	RTS
;Enter with characteristic of multiplier in A,
;mantissa in Y, X pointing to a pair of base page
;locations containing the multiplicand (mantissa in the
;low byte).
;Returns the product in A and Y, same form as the
;multiplier. Leaves X unchanged. Clobbers LOGTMP and
;LOGTMP+1.
MULTIP:	PHA
	EORZX	1	;Compute sign of result,
	STAZ	LOGTMP+1	;save it away.
	PLA
	ANDI	177	;Mask off multiplier sign.
	BEQ	ZEROIN	;If zero, return zero.
	STAZ	LOGTMP
	TYA	;Add the two logarithms.
	CLC
	ADCZX	0
	TAY
	LDAZX	1
	ANDI	177	;If multiplicand is zero,
	BEQ	ZEROIN	;return a zero.
	ADCZ	LOGTMP
	SEC
	SBCI	100	;Correct the XS-64 offset.
	BPL	INSIGN	;Result in range?
	ANDI	100	;No. If underflow,
	BNE	NEGIN	;return zero.
	LDAI	177	;Overflow. Saturate to
	LDYI	377	;highest magnitude.

INSIGN:	ASLA	;Insert the sign of the result.
	ASLZ	LOGTMP+1
	RORA
	RTS

;Inverse function: 2's complement the magnitude part
;of a 15-bit logarithm.
;Enter with characteristic in A, mantissa in Y.
;Returns inverse in the same form. X unchanged.
;Clobbers LOGTMP and LOGTMP+1.
INV:	STYZ	LOGTMP	;Pretty straightforward...
	STAZ	LOGTMP+1
	SEC
	LDAI	0	;Complement the number by
	SBCZ	LOGTMP	;subtracting it from zero.
	TAY
	LDAI	0
	SBCZ	LOGTMP+1
	JMP	INSIGN	;Insert the original sign.
;DAC output table.
   LOC (.∨377)+1	;For start of next page.
VETBL:		;DAC output table.
  0 ↔  20 ↔  26 ↔  33 ↔  37 ↔  43 ↔  46 ↔  50
 52 ↔  54 ↔  56 ↔  57 ↔  60 ↔  62 ↔  63 ↔  64
 65 ↔  66 ↔  67 ↔  70 ↔  71 ↔  72 ↔  73 ↔  74
 75 ↔  76 ↔  76 ↔  77 ↔ 100 ↔ 101 ↔ 102 ↔ 103
104 ↔ 104 ↔ 105 ↔ 106 ↔ 107 ↔ 107 ↔ 110 ↔ 111
112 ↔ 112 ↔ 113 ↔ 114 ↔ 115 ↔ 115 ↔ 116 ↔ 117
117 ↔ 120 ↔ 121 ↔ 121 ↔ 122 ↔ 123 ↔ 124 ↔ 124
125 ↔ 126 ↔ 127 ↔ 127 ↔ 130 ↔ 131 ↔ 131 ↔ 132
133 ↔ 133 ↔ 134 ↔ 135 ↔ 135 ↔ 136 ↔ 136 ↔ 137
140 ↔ 140 ↔ 141 ↔ 142 ↔ 142 ↔ 143 ↔ 143 ↔ 144
145 ↔ 145 ↔ 146 ↔ 146 ↔ 147 ↔ 150 ↔ 150 ↔ 151
151 ↔ 152 ↔ 153 ↔ 153 ↔ 154 ↔ 154 ↔ 155 ↔ 156
156 ↔ 157 ↔ 160 ↔ 160 ↔ 161 ↔ 161 ↔ 162 ↔ 162
163 ↔ 164 ↔ 164 ↔ 165 ↔ 165 ↔ 166 ↔ 166 ↔ 167
167 ↔ 170 ↔ 170 ↔ 171 ↔ 171 ↔ 172 ↔ 172 ↔ 173
173 ↔ 174 ↔ 174 ↔ 175 ↔ 176 ↔ 176 ↔ 177 ↔ 177

200 ↔ 200 ↔ 200 ↔ 201 ↔ 201 ↔ 202 ↔ 203 ↔ 203
204 ↔ 204 ↔ 205 ↔ 205 ↔ 206 ↔ 206 ↔ 207 ↔ 207
210 ↔ 210 ↔ 211 ↔ 211 ↔ 212 ↔ 212 ↔ 213 ↔ 213
214 ↔ 215 ↔ 215 ↔ 216 ↔ 216 ↔ 217 ↔ 217 ↔ 220
221 ↔ 221 ↔ 222 ↔ 223 ↔ 223 ↔ 224 ↔ 224 ↔ 225
226 ↔ 226 ↔ 227 ↔ 227 ↔ 230 ↔ 231 ↔ 231 ↔ 232
232 ↔ 233 ↔ 234 ↔ 234 ↔ 235 ↔ 235 ↔ 236 ↔ 237
237 ↔ 240 ↔ 241 ↔ 241 ↔ 242 ↔ 242 ↔ 243 ↔ 244
244 ↔ 245 ↔ 246 ↔ 246 ↔ 247 ↔ 250 ↔ 250 ↔ 251
252 ↔ 253 ↔ 253 ↔ 254 ↔ 255 ↔ 256 ↔ 256 ↔ 257
260 ↔ 260 ↔ 261 ↔ 262 ↔ 262 ↔ 263 ↔ 264 ↔ 265
265 ↔ 266 ↔ 267 ↔ 270 ↔ 270 ↔ 271 ↔ 272 ↔ 273
273 ↔ 274 ↔ 275 ↔ 276 ↔ 277 ↔ 300 ↔ 301 ↔ 301
302 ↔ 303 ↔ 304 ↔ 305 ↔ 306 ↔ 307 ↔ 310 ↔ 311
312 ↔ 313 ↔ 314 ↔ 315 ↔ 317 ↔ 320 ↔ 322 ↔ 323
325 ↔ 327 ↔ 331 ↔ 334 ↔ 340 ↔ 344 ↔ 351 ↔ 357

   NMI ← START	;Reset??
;Interrupt vectors.
   LOC 177772
	NMI∧377
	(NMI⊗-10)∧377
	START∧377
	(START⊗-10)∧377
	TICK∧377
	(TICK⊗-10)∧377
END